#pragma config(Hubs,  S1, HTMotor,  HTMotor,  HTMotor,  HTMotor)
#pragma config(Hubs,  S2, HTServo,  none,     none,     none)
#pragma config(Sensor, S3,     sensorIR,         sensorI2CCustom)
#pragma config(Sensor, S4,     HTSPB,                sensorI2CCustom9V)
#pragma config(Motor,  mtr_S1_C1_1,     mRight1,       tmotorTetrix, openLoop)
#pragma config(Motor,  mtr_S1_C1_2,     mRight2,       tmotorTetrix, openLoop, encoder)
#pragma config(Motor,  mtr_S1_C2_1,     mArm2,         tmotorTetrix, openLoop)
#pragma config(Motor,  mtr_S1_C2_2,     mSpin,         tmotorTetrix, openLoop)
#pragma config(Motor,  mtr_S1_C3_1,     mLeft1,        tmotorTetrix, openLoop, reversed, encoder)
#pragma config(Motor,  mtr_S1_C3_2,     mLeft2,        tmotorTetrix, openLoop, reversed)
#pragma config(Motor,  mtr_S1_C4_1,     mArm1,         tmotorTetrix, openLoop)
#pragma config(Motor,  mtr_S1_C4_2,     motorK,        tmotorTetrix, openLoop)
#pragma config(Servo,  srvo_S2_C1_1,    servoL1,              tServoStandard)
#pragma config(Servo,  srvo_S2_C1_2,    servoL2,              tServoStandard)
#pragma config(Servo,  srvo_S2_C1_3,    servo3,               tServoStandard)
#pragma config(Servo,  srvo_S2_C1_4,    servo4,               tServoStandard)
#pragma config(Servo,  srvo_S2_C1_5,    servo5,               tServoNone)
#pragma config(Servo,  srvo_S2_C1_6,    servo6,               tServoNone)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

#include "drivers/wiringnxt.h"
#include "drivers/JoystickDriver4278.c"
#include "drivers/autoutils.h"
#include "drivers/hitechnic-irseeker-v2.h"

#define DrTimer T3
#define SeTimer T2

void rbtMoveFd(float inches) {
	int enc = getEncoderByInches(inches); clearEncoders();
	int norm = -1.0*sgn(inches);

	ClearTimer(T1);
	while(leftEncoder < enc || rightEncoder < enc) {
		setLeftMotors (60*norm);
		setRightMotors(60*norm);
		if(time1[T1] > 5000) StopAllTasks();
	}
	setLeftMotors(0); setRightMotors(0);
}

void rbtMoveFdTime(float inches, int msec) {
	int enc = getEncoderByInches(inches); clearEncoders();
	int norm = -1.0*sgn(inches);
	ClearTimer(DrTimer);
	while(leftEncoder < enc && rightEncoder < enc) {
		setLeftMotors (100*norm);
		setRightMotors(100*norm);
		if(time1[DrTimer] > msec) StopAllTasks();
	}
	setLeftMotors(0); setRightMotors(0);
}

void rbtArcLeft(float degs) {
	int enc = getEncoderByInches((2.0*PI*WHEELBASE)*(abs(degs)/360.0));
	clearEncoders();
	setLeftMotors(-1*sgn(degs)*60);
	ClearTimer(T1);
	while(leftEncoder < enc) {
		wait1Msec(10);
		if(time1[T1] > 5000) StopAllTasks();
	}
	setLeftMotors(0);
}

void rbtArcRight(float degs) {
	int enc = getEncoderByInches((2.0*PI*WHEELBASE)*(abs(degs)/360.0));
	clearEncoders();
	setRightMotors(sgn(degs)*60);
	ClearTimer(T1);
	while(rightEncoder < enc) {
		wait1Msec(10);
		if(time1[T1] > 5000) StopAllTasks();
	}
	setRightMotors(0);
}

void rbtTurnRight(float degs) {
	int enc = getEncoderByInches((PI*WHEELBASE)*(abs(degs)/360.0));
	clearEncoders();
	setLeftMotors( -1*sgn(degs)*60);
	setRightMotors(sgn(degs)*60);
	ClearTimer(T1);
	while(leftEncoder < enc) {
		wait1Msec(10);
		if(time1[T1] > 5000) StopAllTasks();
	}
	setLeftMotors(0); setRightMotors(0);
}

void rbtTurnLeft(float degs) {
	int enc = getEncoderByInches((PI*WHEELBASE)*(abs(degs)/360.0));
	clearEncoders();
	setLeftMotors(sgn(degs)*60);
	setRightMotors(-1*sgn(degs)*60);
	ClearTimer(T1);
	while(leftEncoder < enc) {
		wait1Msec(10);
		if(time1[T1] > 5000) StopAllTasks();
	}
	setLeftMotors(0); setRightMotors(0);
}

void dumpArm() {
	setArmMotors(50);
	wait1Msec(1550);
	setArmMotors(0);
	wait1Msec(400);
	setArmMotors(-50);
	wait1Msec(1100);
	setArmMotors(0);
}

void pause() {wait1Msec(175);}

bool pathClear(float dist){
	pause();
	float read = 0;
	for(int i=0;i<10;i++){read+=(analogRead(A3)*0.4);wait1Msec(7);}
	nxtDisplayBigTextLine(3,"%f", read/10.0);
	return ((read/10)<dist?false:true);
}

void normstop() {while(true) wait1Msec(100);}
void estop() {StopAllTasks();}

/*!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
 * Start Autonomous Fucntions
 *!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!*/

void rightBridge(float bridgeDist){
	rbtMoveFd(bridgeDist); pause();
	rbtArcRight(-90); pause();
	rbtMoveFd(22); pause();
	rbtArcRight(-94); pause();
	rbtMoveFdTime(30, 3000);
	normstop();
}

void leftBridge(float bridgeDist){
	rbtMoveFd(bridgeDist); pause();
	rbtArcLeft(88); pause();
	rbtMoveFd(22); pause();
	rbtArcLeft(94); pause();
	rbtMoveFdTime(30, 3000);
	normstop();
}

void farLeftBridge(float bridgeDist){
	rbtMoveFd(-bridgeDist); pause();
	rbtArcRight(90); pause();
	rbtMoveFd(-22); pause();
	rbtArcLeft(-90); pause();
	rbtMoveFdTime(45, 3000);
	normstop();
}

void farRightBridge(float bridgeDist){
	rbtMoveFdTime(-bridgeDist, 3000); pause();
	rbtArcLeft(-89); pause();
	rbtMoveFd(-22); pause();
	rbtArcRight(90); pause();
	rbtMoveFdTime(45, 3000);
	normstop();
}

/****************************************************************************/

void crateOne(){
	rbtMoveFd(7); pause();
	rbtArcRight(-86.5); pause();
	rbtMoveFdTime(3, 1000); pause();pause();
	dumpArm();
	rbtMoveFd(-0.756);
	rbtArcRight(95.5); pause();
	if(pathClear(30)) rightBridge(15.5);
	else farLeftBridge(39.5);
}

void crateTwo(){
//	rbtMoveFd(1.5); pause();
	rbtArcRight(-92); pause();
	rbtMoveFdTime(3, 1000); pause();pause();
	dumpArm();
	rbtMoveFd(-0.756);
	rbtArcRight(93.5); pause();
	if(pathClear(50)) rightBridge(25);
	else farLeftBridge(30);
}

void crateThree(){
	rbtArcLeft(45); pause();
	//rbtMoveFd(1.5); pause();
	rbtArcRight(-90); pause();
	rbtMoveFdTime(2, 750); pause();pause();
	dumpArm();
	rbtMoveFd(-0.756);
	rbtArcLeft(-87); pause();
	if(pathClear(57)) leftBridge(23);
	else farRightBridge(29.5);
}

void crateFour(){
	rbtMoveFd(4.5); pause();
	rbtArcRight(-42); pause();
	rbtMoveFdTime(2.4, 1000); pause(); pause();
	dumpArm();
	rbtMoveFd(-0.5); pause();
	rbtArcLeft(-87); pause();
	if(pathClear(40)) leftBridge(14);
	else farRightBridge(38);
}

void initializeRobot(){
	unlockArmMotors();
}

int crateButtons() {
	int crate = 0;
	while(!nNxtButtonPressed == BTN_CENTER) {
		if(crate > 0) nxtDisplayTextLine(3, "Crate? %i", crate);
		else nxtDisplayTextLine(3, "Crate? IR");
		if(nNxtButtonPressed == BTN_LEFT)  {crate--; while(nNxtButtonPressed == BTN_LEFT);}
		if(nNxtButtonPressed == BTN_RIGHT) {crate++; while(nNxtButtonPressed == BTN_RIGHT);}
		if(crate < 0) crate = 4; if(crate > 4) crate = 0;
	}
	return crate;
}
long nNumbCyles;
long nInits = 0;
string sTextLines[8];

void displayText(int nLineNumber, const string cChar, int nValueDC, int nValueAC)
{
  string sTemp;

  StringFormat(sTemp, "%4d  %4d", nValueDC, nValueAC);
  // Check if the new line is the same as the previous one
  // Only update screen if it's different.
  if (sTemp != sTextLines[nLineNumber])
  {
    string sTemp2;

    sTextLines[nLineNumber] = sTemp;
    StringFormat(sTemp2, "%s:  %s", cChar, sTemp);
    nxtDisplayTextLine(nLineNumber, sTemp2);
  }
}

void crateSelect(){
	int crt;
	int dirIR, strIR;
	float in;
	HTIRS2readEnhanced(sensorIR, dirIR, strIR);
	clearEncoders();
	if((dirIR == 4 && strIR >= 90) || (dirIR == 5 && strIR <= 85)){
		crateFour();
	}else if(dirIR == 5 && strIR >= 85){
		crateThree();
	}else{
		rbtArcLeft(43.4);
		pause();
		rbtMoveFd(18.5);
		pause();
		HTIRS2readEnhanced(sensorIR, dirIR, strIR);
		nxtDisplayBigTextLine(3, "%d, %d", dirIR, strIR);
		if(dirIR == 3 && strIR > 70){
			crateTwo();
		}else{
			crateOne();
		}
	}
	estop();
}

/*!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
 * End Autonomous Fucntions
 *!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!*/

task main() {
	displayDiagnostics();
	initializeRobot();
	waitForStart();
	wait1Msec(11000);
	crateSelect();
}
